#include "vec3.h"


float Vec3::Length() const
{
	return sqrtf( LengthSqr() );
}


void Vec3::Normalize()
{
	float l = Length();
	//TODO add assert( l > 0.0f );
	l = 1.0f / l;

	x *= l;
	y *= l;
	z *= l;
}


void Vec3::SetCross( const Vec3 &v1, const Vec3 &v2 )
{
	//TODO add assert( this != &v1 );
	//TODO add assert( this != &v2 );

	x = v1.y * v2.z - v1.z * v2.y;
	y = v1.z * v2.x - v1.x * v2.z;
	z = v1.x * v2.y - v1.y * v2.x;
}


void Vec3::SetReflectedVector( const Vec3 &normal, const Vec3 &vector )
{
	const float f = 2.0f * ( vector * normal );

	x = vector.x - normal.x * f;
	y = vector.y - normal.y * f;
	z = vector.z - normal.z * f;
}


void Vec3::SetLerp( const Vec3 &v1, const Vec3 &v2, float t )
{
	if( t <= 0.0f )
	{
		*this = v1;
		return;
	}
	else if( t >= 1.0f )
	{
		*this = v2;
		return;
	}

	x = v1.x * ( 1.0f - t ) + v2.x * t;
	y = v1.y * ( 1.0f - t ) + v2.y * t;
	z = v1.z * ( 1.0f - t ) + v2.z * t;
}


void Vec3::SetSLerp( const Vec3 &v1, const Vec3 &v2, float t )
{
	float omega, cosom, sinom, scale0, scale1;

	if( t <= 0.0f )
	{
		*this = v1;
		return;
	}
	else if( t >= 1.0f )
	{
		*this = v2;
		return;
	}

	cosom = v1 * v2;
	assert( cosom <= 1.0f && cosom >= -1.0f );
	if( ( 1.0f - cosom ) > 1e-5f )
	{
		omega = acos( cosom );
		sinom = sinf( omega );
		scale0 = sinf( ( 1.0f - t ) * omega ) / sinom;
		scale1 = sinf( t * omega ) / sinom;
	}
	else
	{
		scale0 = 1.0f - t;
		scale1 = t;
	}

	x = v1.x * scale0 + v2.x * scale1;
	y = v1.y * scale0 + v2.y * scale1;
	z = v1.z * scale0 + v2.z * scale1;
}


void Vec3::RotateAroundX( float fAngle )
{
	const float fY = y;
	const float fZ = z;

	const float fSinA = sin( fAngle );
	const float fCosA = cos( fAngle );

	y = ( fY * fCosA ) - ( fZ * fSinA );
	z = ( fZ * fCosA ) + ( fY * fSinA );
}


void Vec3::RotateAroundY( float fAngle )
{
	const float fX = x;
	const float fZ = z;

	const float fSinA = sin( fAngle );
	const float fCosA = cos( fAngle );

	x = ( fX * fCosA ) - ( fZ * fSinA );
	z = ( fZ * fCosA ) + ( fX * fSinA );
}


void Vec3::RotateAroundZ( float fAngle )
{
	const float fX = x;
	const float fY = y;

	const float fSinA = sin( fAngle );
	const float fCosA = cos( fAngle );

	x = ( fX * fCosA ) - ( fY * fSinA );
	y = ( fY * fCosA ) + ( fX * fSinA );
}
